import sys
import rclpy
from rclpy.node import Node
from mavros_msgs.srv import SetMode  # 这是 MAVROS2 中的服务类型，用于设置无人机模式
import time  # 用于控制重试之间的时间间隔（例如，等待 5 秒后重试）

# 自定义节点类；
class ModeSwitchClient(Node):
    def __init__(self):
        super().__init__('mavros_mode_switch_client')  # 传入节点名称
        self.get_logger().info("客户端已创建！(mode)")

        # 创建与 MAVROS 设置模式的客户端，订阅'/mavros/set_mode' 服务
        self.client = self.create_client(SetMode, '/mavros/set_mode')
        while not self.client.wait_for_service(timeout_sec=3.0):
            self.get_logger().info('SetMode Service not available, waiting again...')
            time.sleep(3)  # 给服务一些时间准备

    def send_request(self):
        request = SetMode.Request()  # 创建请求消息
        request.custom_mode = "AUTO"  # 设置无人机模式为 "OFFBOARD"
        return self.client.call_async(request)

def main():
    # 初始化 ROS2 客户端；
    rclpy.init()
    mode = ModeSwitchClient()

    # 进行重试，直到服务成功
    success = False
    while not success:
        future = mode.send_request()
        rclpy.spin_until_future_complete(mode, future)

        if future.result() is not None and future.result().mode_sent:  # 检查模式是否成功设置
            mode.get_logger().info('Mode switch successful!')
            success = True
        else:
            mode.get_logger().warn('Mode switch failed, retrying in 5 seconds...')
            time.sleep(5)  # 等待 5 秒后重试

    mode.get_logger().info('Mode successfully set to AUTO.')
    
    # 资源释放；
    mode.destroy_node()  # 销毁节点
    rclpy.shutdown()

if __name__ == '__main__':
    main()
